import os
import launch
import launch_ros
import launch.event_handlers
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration("use_sim_time", default="True")
    map_file = os.path.join(
        get_package_share_directory("nav_pkg"), "maps", "map.yaml"
    )
    # 创建map_server节点
    map_server_cmd = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map_file},
                    {'topic': 'map'},
                    {'frame_id': 'map'},
                    {'output': 'screen'},
                    {'use_sim_time': True}]
    )
    lifecycle_nodes = [
        'map_server',
        'rviz_planner']
    lifecycle_manager_cmd = launch_ros.actions.Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_planner',
            output='screen',
            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
            parameters=[{'use_sim_time': use_sim_time},
                        {'autostart': True},
                        {'node_names': lifecycle_nodes}])
    
    
    rviz_file = os.path.join(
        get_package_share_directory("rviz_planner"), "rviz", "planner.rviz"
    )
    rviz_cmd = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz_file],
    )
    
    # 规划路径
    planner_cmd = Node(
        package='rviz_planner',
        executable='rviz_planner',
        name='rviz_planner',
        output='screen'
    )

    ld = LaunchDescription()
    ld.add_action(map_server_cmd)
    ld.add_action(planner_cmd)
    ld.add_action(lifecycle_manager_cmd)

    ld.add_action(rviz_cmd)
    
    return ld
